/*
The MIT License (MIT)

Copyright (c) 2013 Mike Dapiran, Brian May, Richard Pospesel, and Bert Wierenga

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software 
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#include "RigidBody.h"
#include "CollisionShape.h"
#include "TriggerVolume.h"

#include "../Messages/ApplyTransformMessage.h"
#include "../Messages/ApplyVelocityMessage.h"
#include "../Messages/ApplyImpulseMessage.h"
#include "../Messages/SetForceMessage.h"
#include "../Messages/SetFrictionMessage.h"
#include "../Messages/SetDampeningMessage.h"
#include "../Messages/SetMaxSpeedMessage.h"
#include "../Messages/SetGravityEnabledMessage.h"
#include "../Messages/SetGroupMaskMessage.h"
#include "../Messages/SetCollisionActivity.h"

#include "../DataStructures/HogsheadMotionState.h"
#include "../Directors/BulletPhysicsDirector.h"
#include "../Hogshead.BulletPhysics.h"
namespace Hogshead
{
	namespace BulletPhysics
	{
		CollisionObjectCPP(Hogshead::BulletPhysics::RigidBody);
		RigidBody::RigidBody()
		{
			_bullet_director = NULL;	// director pointer
			_rigid_body = NULL;		// btRigidBody pointer
			_collision_shape = NULL;	// btCollisionShape pointer
			_motion_state = NULL;	// motionstate pointer (synchronizes RigidBody transform with bullet's internal transforms)
			
			_gravity_enabled = true;
			
			_type_string = "";
			_mass = 0.0f;
			_friction = 0.0f;
			_restitution = 0.0f;

			_type = RigidBodyType::Static;
			_angular_factor.set(1,1,1);
			_linear_factor.set(1,1,1);
			_max_speed = Float::Infinity;

			_max_piecewise_speed.set(Float::Infinity, Float::Infinity, Float::Infinity);

			_pf.registerVariable(ParameterField::String, "rigidBodyType", &_type_string);
			_pf.registerVariable(ParameterField::Float, "mass", &_mass);
			_pf.registerVariable(ParameterField::Vector3, "angularFactor", &_angular_factor);
			_pf.registerVariable(ParameterField::Vector3, "linearFactor", &_linear_factor);
			_pf.registerVariable(ParameterField::Float, "maxSpeed", &_max_speed);
			_pf.registerVariable(ParameterField::Vector3, "maxPiecewiseSpeed", &_max_piecewise_speed);
			_pf.registerVariable(ParameterField::Float, "restitution", &_restitution);
			_pf.registerVariable(ParameterField::Float, "friction", &_friction);

			this->registerMessageCallback<ApplyTransformMessage>(RigidBody::handle_apply_transform_message);
			this->registerMessageCallback<ApplyVelocityMessage>(RigidBody::handle_apply_velocity_message);
			this->registerMessageCallback<ApplyImpulseMessage>(RigidBody::handle_apply_impulse_message);
			this->registerMessageCallback<SetForceMessage>(RigidBody::handle_set_force_message);
			this->registerMessageCallback<SetFrictionMessage>(RigidBody::handle_set_friction_message);
			this->registerMessageCallback<SetRestitutionMessage>(RigidBody::handle_set_restitution_message);
			this->registerMessageCallback<SetDampeningMessage>(RigidBody::handle_set_dampening_message);
			this->registerMessageCallback<SetMaxSpeedMessage>(RigidBody::handle_set_max_speed_message);
			this->registerMessageCallback<SetGravityEnabledMessage>(RigidBody::handle_set_gravity_enabled_message);
			this->registerMessageCallback<SetGroupMaskMessage>(RigidBody::handle_set_group_mask);
			this->registerMessageCallback<SetCollisionActivity>(RigidBody::handle_set_activity);

			//this->registerMessageCallback<CollisionMessage>(RigidBody::handle_collision_message);
		}
		
		RigidBody::~RigidBody()
		{
			
		}

		IComponent* RigidBody::clone() const
		{
			RigidBody* result = Factory::acquire<RigidBody>();

			result->_bullet_director = NULL;
			result->_rigid_body = NULL;
			result->_motion_state = NULL;

			result->_group = _group;
			result->_mask = _mask;

			result->_type_string = _type_string;
			result->_mass = _mass;
			result->_friction = _friction;
			result->_restitution = _restitution;
			
			result->_type = _type;
			result->_angular_factor = _angular_factor;
			result->_linear_factor  = _linear_factor;
			result->_max_speed = _max_speed;

			result->_gravity_enabled = _gravity_enabled;

			return result;
		}

		void RigidBody::initialize()
		{
			if(this->getParentGameObject() == this->getParentScene()->getRootGameObject())
				error("RigidBodies cannot exist on the root game object");

			_bullet_director  =  (BulletPhysicsDirector*)this->getParentScene()->getDirector("BulletPhysicsDirector");
			if(!_bullet_director)
				error("RigidBody requires a BulletPhysicsDirector");

			if(_parent->getComponentsOfType<RigidBody>().length() != 1)
				error(StringBuilder() << "RigidBody on GameObject " << this->getParentGameObject()->name() << " has multiple sibling RigidBodies");

			if(_parent->getComponentOfType<TriggerVolume>())
				error(StringBuilder() << "RigidBody on GameObject " << this->getParentGameObject()->name() << " has sibling TriggerVolume");


			// get the first CollisionShape 
			Array1D<CollisionShape*>& shapes = _parent->getComponentsOfType<CollisionShape>();
			if(shapes.length() == 1)
			{
				CollisionShape* shape = shapes[0];
				//setup our motion state
				_motion_state = new HogsheadMotionState(this->_parent);

				// build initial parameters
				btVector3 fallInertia(0.0f, 0.0f, 0.0f);
				_collision_shape = shape->getBulletCollisionShape();
				_collision_shape->setMargin(0.025);
				_collision_shape->calculateLocalInertia(_mass, fallInertia);

				btRigidBody::btRigidBodyConstructionInfo _construction_info(_mass, _motion_state, _collision_shape, fallInertia);
				_rigid_body = new btRigidBody(_construction_info);

				/** HERE WE CAN SETUP PARSED PARAMETERS LIKE FRICTION, ETC **/

				_rigid_body->setAngularFactor(_angular_factor);
				_rigid_body->setLinearFactor(_linear_factor);
				_rigid_body->setFriction(_friction);
				_rigid_body->setRestitution(_restitution);

				// if this is true, then we know it HAS to be static
				if((shape->_collision_shape_types & CollisionShapeType::Plane) && _type != RigidBodyType::Static)
				{
					error("Problem with RigidBody construction: CollisionShape contains a Plane collider which must be Static, but rigidBodyType is not Static");
				}

				_rigid_body->setCollisionFlags(0);

				switch(_type)
				{
				case RigidBodyType::Static:
					_rigid_body->setCollisionFlags(_rigid_body->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT);
					break;
				case RigidBodyType::Kinematic:
					_rigid_body->setCollisionFlags(_rigid_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT );
					_rigid_body->setActivationState(DISABLE_DEACTIVATION);
					_rigid_body->activate(true);
					break;
				case RigidBodyType::Dynamic:
					_rigid_body->activate(true);

					break;
				}
				
				_bullet_director->addCollisionObject(this, _group, _mask);
			}
			else
				error("RigidBody component must have only 1 sibling CollisionShape component");

			this->_previous_mask = this->_mask;
		}

		void RigidBody::finalize()
		{
			ICollisionObject::finalize();

			if(_bullet_director)
				_bullet_director->removeCollisionObject(this);


			_bullet_director = NULL;	// director pointer
			if(_rigid_body)
				delete _rigid_body;
			_rigid_body = NULL;		// btRigidBody pointer
			if(_collision_shape)
				CollisionShape::deleteBtCollisionShape(_collision_shape);
			_collision_shape = NULL;
			if(_motion_state)
				delete _motion_state;
			_motion_state = NULL;	// motionstate pointer (synchronizes RigidBody transform with bullet's internal transforms)
		
			// collision filtering
			_group = 1;
			_mask = -1;

			_type_string = "";
			_mass = 0.0f;
			_friction = 0.0f;
			_restitution = 0.0f;

			_type = RigidBodyType::Static;
			_angular_factor.set(1,1,1);
			_linear_factor.set(1,1,1);
			_max_speed = Float::Infinity;
			_gravity_enabled = true;
		}

		Vector3 RigidBody::getVelocity() const
		{
			return _rigid_body->getLinearVelocity();
		}

		Vector3 RigidBody::getPosition() const
		{
			
			return _rigid_body->getWorldTransform().getOrigin();
		}

		Quaternion RigidBody::getOrientation() const
		{
			return _rigid_body->getWorldTransform().getRotation();
		}

		IComponent* RigidBody::readXML(const String& in_xml)
		{
			// read in stuff frm parameter field
			IComponent::readXML(in_xml);

			// defaults to Static
			Hogshead::BulletPhysics::rigid_body_enum_table->tryGetValue(_type_string, _type);
			
			return this;
		}

		void RigidBody::update(const GameTimer& in_gt)
		{

			//hout << this->getParentGameObject()->name() << " velocity: " << _rigid_body->getLinearVelocity() << endl;
		}	
		
		void RigidBody::handle_collision_message(IMessageReceiver* receiver, IMessage* message)
		{
/*
			RigidBody* me = (RigidBody*)receiver;
			if(me->_type == RigidBodyType::Kinematic)
			{
				if(CollisionMessage* cm = message->as<CollisionMessage>())
				{
					if(RigidBody* other = cm->collided_with->as<RigidBody>())
					{
						if(other->_type == RigidBodyType::Static)
						{
							// move rigid bodies game object back
							hout << "my contact: " <<  cm->my_collision_points[0] << endl;
							hout << "their contact: " << cm->their_collision_points[0] << endl;

							Vector3 offset = cm->their_collision_points[0] - cm->my_collision_points[0];
							offset *= 1.01;
							hout << "offset: " << offset << endl;
							btTransform desired_transform = me->_rigid_body->getWorldTransform();
							btVector3 original_position = desired_transform.getOrigin();
							hout << original_position << endl;
							btVector3 desired_position = original_position + offset;
							desired_transform.setOrigin(desired_position);
							me->_rigid_body->setWorldTransform(desired_transform);
							me->_motion_state->setWorldTransform(desired_transform);
							hout << me->getParentGameObject()->getTransform().getWorldPosition() << endl;
						}
					}
				}
			}
*/
		}

		void RigidBody::handle_apply_transform_message(IMessageReceiver* receiver,IMessage* message)
		{
			RigidBody* me = (RigidBody*)receiver;

			if(me->_type == RigidBodyType::Dynamic)
			{
				// set transform
				if(const ApplyTransformMessage* atm = message->as<ApplyTransformMessage>())
				{
					btTransform& t = me->_rigid_body->getWorldTransform();
					// set translation
					if(atm->_position_set)
					{
						// global position
						if(atm->_position_global)
							t.setOrigin(atm->_position);
						// position relative to us
						else
							t.setOrigin(t.getOrigin() + t.getBasis() * atm->_position);
					}
					// set rotation
					if(atm->_rotation_set)
					{
						// global orientation
						if(atm->_rotation_global)
							t.setRotation(atm->_rotation);
						// orientation relative to us
						else
							t.setRotation(t.getRotation() * atm->_rotation);
					}

					me->_rigid_body->setWorldTransform(t);
					me->_rigid_body->activate(true);
				}
			}
		}
		void RigidBody::handle_apply_velocity_message(IMessageReceiver* receiver,IMessage* message)
		{
			RigidBody* me = (RigidBody*)receiver;
			if(me->_type == RigidBodyType::Dynamic)
			{
				if(const ApplyVelocityMessage* avm = message->as<ApplyVelocityMessage>())
				{
					btTransform& t = me->_rigid_body->getWorldTransform();

					// set linear velocity
					if(avm->_linear_set)
					{
						// global space
						if(avm->_linear_global)
							me->_rigid_body->setLinearVelocity(avm->_linear_velocity);
						// local spacce
						else
							me->_rigid_body->setLinearVelocity(t.getBasis() * avm->_linear_velocity);
					}
					// set angular velocity
					if(avm->_angular_set)
					{
						// global space
						if(avm->_angular_global)
							me->_rigid_body->setAngularVelocity(avm->_angular_velocity);
						// local space
						else
							me->_rigid_body->setAngularVelocity(t.getBasis() * avm->_angular_velocity);
					}

					me->_rigid_body->activate(true);
				}
			}
		}

		void RigidBody::handle_apply_impulse_message(IMessageReceiver* receiver,IMessage* message)
		{
			RigidBody* me = (RigidBody*)receiver;
			if(me->_type == RigidBodyType::Dynamic)
			{
				if(const ApplyImpulseMessage* aim = message->as<ApplyImpulseMessage>())
				{
					btTransform& t = me->_rigid_body->getWorldTransform();
					// set linear impulse
					if(aim->_linear_set)
					{
						// global space
						if(aim->_linear_global)
							me->_rigid_body->applyCentralImpulse(aim->_linear_impulse);
						// local spacce
						else
							me->_rigid_body->applyCentralImpulse(t.getBasis() * aim->_linear_impulse);
					}
					// set angular impulse
					if(aim->_angular_set)
					{
						// global space
						if(aim->_angular_global)
							me->_rigid_body->applyTorqueImpulse(aim->_angular_impulse);
						// local space
						else
							me->_rigid_body->applyTorqueImpulse(t.getBasis() * aim->_angular_impulse);
					}

					me->_rigid_body->activate(true);
				}
			}
		}
		void RigidBody::handle_set_force_message(IMessageReceiver* receiver,IMessage* message)
		{
			RigidBody* me = (RigidBody*)receiver;
			if(me->_type == RigidBodyType::Dynamic)
			{
				if(const SetForceMessage* sfm = message->as<SetForceMessage>())
				{
					// set force
					if(sfm->_force_set)
					{
						// global
						if(sfm->_force_global)
							me->_global_force = sfm->_force;
						// local
						else
							me->_local_force = sfm->_force;
					}
					// set torque
					if(sfm->_torque_set)
					{
						// global
						if(sfm->_torque_global)
							me->_global_torque = sfm->_torque;
						else
							me->_local_torque = sfm->_torque;
					}	

					me->_rigid_body->activate(true);
				}
			}	
		}
		void RigidBody::handle_set_friction_message(IMessageReceiver* receiver,IMessage* message)
		{
			RigidBody* me = (RigidBody*)receiver;
			if(const SetFrictionMessage* sfm = message->as<SetFrictionMessage>())
			{
				me->_friction = sfm->_friction;
				me->_rigid_body->setFriction(sfm->_friction);
				me->_rigid_body->activate(true);
			}
		}

		void RigidBody::handle_set_max_speed_message(IMessageReceiver* receiver,IMessage* message)
		{
			RigidBody* me = (RigidBody*)receiver;
			if(const SetMaxSpeedMessage* smsm = message->as<SetMaxSpeedMessage>())
			{
				if(smsm->_max_speed_set)
				{
					me->_max_speed = smsm->_max_speed;
					me->_max_piecewise_speed.set(Float::Infinity, Float::Infinity, Float::Infinity);
				}
				else if(smsm->_max_piecewise_speed_set)
				{
					me->_max_piecewise_speed = smsm->_max_piecewise_speed;
					me->_max_speed = Float::Infinity;
				}
				me->_rigid_body->activate(true);
			}
		}
		void RigidBody::handle_set_restitution_message(IMessageReceiver* receiver,IMessage* message)
		{
			RigidBody* me = (RigidBody*)receiver;
			if(const SetRestitutionMessage* srm = message->as<SetRestitutionMessage>())
			{
				me->_rigid_body->setRestitution(srm->_restitution);
				me->_rigid_body->activate(true);
			}
		}
		void RigidBody::handle_set_dampening_message(IMessageReceiver* receiver,IMessage* message)
		{
			RigidBody* me = (RigidBody*)receiver;
			if(me->_type == RigidBodyType::Dynamic)
			{
				if(const SetDampeningMessage* sdm = message->as<SetDampeningMessage>())
				{
					if(sdm->_linear_set)
						me->_rigid_body->setLinearFactor(sdm->_linear_dampening);
					if(sdm->_angular_set)
						me->_rigid_body->setAngularFactor(sdm->_angular_dampening);
					me->_rigid_body->activate(true);
				}
			}
		}

		void RigidBody::handle_set_gravity_enabled_message(IMessageReceiver* receiver, IMessage* message)
		{
			RigidBody* _this = (RigidBody*)receiver;
			if(_this->_type == RigidBodyType::Dynamic)
			{
				if(SetGravityEnabledMessage* sgem = message->as<SetGravityEnabledMessage>())
					_this->_gravity_enabled = sgem->_gravity_enabled;
			}
		}

		void RigidBody::onSetActive()
		{
			if(_bullet_director) _bullet_director->addCollisionObject(this, _group, _mask);
			
		}

		void RigidBody::onSetInactive()
		{
			if(_bullet_director) _bullet_director->removeCollisionObject(this);
		}

		void RigidBody::handle_set_group_mask(IMessageReceiver* me, IMessage* msg)
		{
			if(const SetGroupMaskMessage* sgmm = msg->as<SetGroupMaskMessage>())
			{
				RigidBody* _this = (RigidBody*)me;
				if(_this->_bullet_director) _this->_bullet_director->removeCollisionObject(_this);

				if(sgmm->_change_mask & MaskChanged)
				{
					_this->_mask = sgmm->_mask;
				}
				if(sgmm->_change_mask & GroupChanged)
				{
					_this->_group = sgmm->_group;
				}

				if(_this->_bullet_director) _this->_bullet_director->addCollisionObject(_this, _this->_group, _this->_mask);
			}
		}

		Vector3 RigidBody::getExtents() const
		{
			btVector3 mins, maxs;
			_rigid_body->getAabb(mins, maxs);
			float margin = _collision_shape->getMargin() * 2;
			Vector3 result = maxs - mins;
			result = result - Vector3(margin, margin, margin);
			return result;
		}

		void RigidBody::handle_set_activity(IMessageReceiver* me, IMessage* msg)
		{
			if(const SetCollisionActivity* sca = msg->as<SetCollisionActivity>())
			{
				RigidBody* _this = (RigidBody*)me;

				if(_this->_bullet_director) _this->_bullet_director->removeCollisionObject(_this);

				if(sca->_activity)
				{
					_this->_mask = _this->_previous_mask;
				}
				else
				{
					_this->_previous_mask = _this->_mask;
					_this->_mask = 0;
				}

				if(_this->_bullet_director) _this->_bullet_director->addCollisionObject(_this, _this->_group, _this->_mask);
			}
		}
	}
}